SKOLTECH, Experimental Data Processing

Final Project

Evgenii Safronov, Mikhail Kurenkov, Taras Melnik

Read data

In [2]:
mat_contents = sio.loadmat('data_odometry.mat')
X = np.array(mat_contents["X"][0])
Y = np.array(mat_contents["Y"][0])
V_right = np.array(mat_contents["V_right"].transpose()[0])
V_left = np.array(mat_contents["V_left"].transpose()[0])
V_odom = (V_right + V_left) / 2

Visualization

In [3]:
fig, ax = plt.subplots(1, 1, figsize=(6, 3), dpi=600)
ax.set_title("GPS trajectory")
ax.plot(X, Y)
ax.set_ylabel("Y")
ax.set_xlabel("X");
In [4]:
fig, ax = plt.subplots(1, 2, figsize=(6, 3), dpi=600)
ax[0].plot(X);ax[0].set_title("X from GPS");ax[0].set_ylabel("X");ax[0].set_xlabel("points");ax[1].plot(Y);ax[1].set_title("Y from GPS");ax[1].set_ylabel("Y");ax[1].set_xlabel("points");
plt.tight_layout();

Odometry data

In [5]:
fig, ax = plt.subplots(1, 1, figsize=(6, 3), dpi=600)
ax.patch.set_facecolor('grey');ax.set_title("Velocity");ax.plot(V_left,color='white',label=r'$V_{left}$');ax.plot(V_odom,color='blue',label=r"$V_{odometry}$");ax.plot(V_right,color='red',label=r'$V_{right}$');ax.set_xlabel("points");ax.set_ylabel("V");
ax.legend(loc='best');

Calculation of GPS velocity

$$v_i = \frac{x_{i+1} - x_{i-1}}{2 t}$$

In [6]:
def velocity_from_traj(x, t):
    v = (np.roll(x, -1) - np.roll(x, 1)) / 2 / t
    v[-1] = v[-2]
    v[0] = v[1]
    return v

t = 0.05
V_x = velocity_from_traj(X, t)
V_y = velocity_from_traj(Y, t)
V_GPS = (V_x ** 2 + V_y ** 2) ** 0.5
In [7]:
fig, ax = plt.subplots(1, 3, figsize=(6, 3), dpi=600);
ax[0].plot(V_x,label='$V_x$');ax[1].set_title('GPS');ax[0].set_ylabel("Velocity");ax[1].plot(V_y,label="$V_y$");ax[2].plot(V_GPS,label=r"$\left|V\right|$");
for axe in ax: 
    axe.legend(loc='lower center');axe.set_xlabel("points");
fig.tight_layout();

Simple 1-dim linear regression

In [8]:
def LR(y,x):
    N = len(y)
    β = inv(x.dot(x.transpose())).dot(x.dot(y.transpose()))
    return β
def RMS(y,x,β):
    return (np.sum((y-x.transpose().dot(β))**2)/ (len(y)-1)) ** 0.5
In [9]:
y = V_GPS
x = V_odom
In [10]:
β = LR(y,np.array([np.ones_like(x),x]))
print(β)
rms = RMS(y, np.array([np.ones_like(x),x]), β)
print(rms)
[ 2.01151947  0.79883512]
0.0851295301498

$$V_{GPS} = 2.01 + 0.8 \cdot V_{odometry}$$ $$RMS = 0.085$$

In [11]:
fig, ax = plt.subplots(1, 1, figsize=(6, 3), dpi=600);ax.scatter(x, y, s=0.3,label='GPS data');x_line = x;y_line = np.array([np.ones_like(x),x]).transpose().dot(β);ax.plot(x_line, y_line, linewidth=1, color="r",label='linear fit');ax.legend(loc='best');ax.set_xlabel("$V_{odom}$");ax.set_ylabel("$V_{GPS}$");
ax.set_title("Linear fitting with RMS = %.3f" % rms);

Let's provide a little bit of noise to the GPS data and try to recover $C_0$, $C_1$ again

Noisify data

In [12]:
sigma_n_2 = 3 ** 2
Xn = X + np.random.normal(0, sigma_n_2 ** 0.5, X.shape[0])
Yn = Y + np.random.normal(0, sigma_n_2 ** 0.5, Y.shape[0])
In [13]:
fig, ax = plt.subplots(1, 2, figsize=(6, 3), dpi=600);ax[0].plot(Xn,label=r'$X_{GPS}$, noisy');ax[0].plot(X, label=r'$X_{GPS}$');ax[0].set_title("X");ax[0].set_ylabel("coordinate");ax[0].legend(loc='lower right');ax[1].plot(Yn,label=r'$Y_{GPS}$, noisy');ax[1].plot(Y, label=r'$Y_{GPS}$');ax[1].set_title("Y");ax[1].legend(loc='lower left');
for axe in ax:
    axe.set_xlabel("points")
plt.tight_layout();

Use Kalman filter to reduce introduced noise

Forward Kalman...

In [17]:
Xk, _, P, _ = kalman(X_0, P_0, z[2:], Φ, H, R, Q)
In [21]:
 

Huge oscillations at first points $\Rightarrow$ need for backward smoothing

In [23]:
Xs, _, _ = backward_smoothing(Xk, P, Q, Φ)
In [24]:
 

Now we can extract velocity

In [26]:
 

We got approximate measurements of V. Let's see how they will affect linear regression accuracy

In [27]:
y = V_gps_noisy[:,0]
x = V_odom[2:]
In [28]:
β = LR(y,np.array([np.ones_like(x),x]))
print(β)
rms = RMS(y, np.array([np.ones_like(x),x]), β)
print(rms)
[ 2.75332442  0.75409233]
0.291461948781

$$V_{GPS} = 2.75 + 0.75 \cdot V_{odometry}$$ $$RMS = 0.29$$

In [29]:
fig, ax = plt.subplots(1, 1, figsize=(6, 3), dpi=600)
ax.scatter(x, y, s=0.3,label='noisy data');ax.scatter(x, V_GPS[2:], s=0.1, label='original');
x_line = x;y_line = np.array([np.ones_like(x),x]).transpose().dot(β);
ax.plot(x_line, y_line, linewidth=1, color="r",label='linear fit');ax.set_xlabel("$V_{odom}$");ax.set_ylabel("$V_{GPS}$");ax.legend(loc='best')
ax.set_title("Linear fit with RMS = %.2f" % rms);

Finally, let's remove ends

In [30]:
interval = range(50, 450)
V_x_noisy2 = V_x_noisy[interval]
V_y_noisy2 = V_y_noisy[interval]
V_gps_noisy2 = V_gps_noisy[interval]
In [31]:
 

Results for linear regression are a way closer to GPS without noise

In [32]:
y = V_gps_noisy2[:,0]
x = V_odom[interval]
In [33]:
β = LR(y,np.array([np.ones_like(x),x]))
print(β)
rms = RMS(y, np.array([np.ones_like(x),x]), β)
print(rms)
[ 2.09250348  0.79628892]
0.196604597478

$$V_{GPS} = 2.09 + 0.8 \cdot V_{odometry}$$ $$RMS = 0.2$$

In [35]:
fig, ax = plt.subplots(1, 1, figsize=(6, 3), dpi=600)
ax.scatter(x, y, s=0.1,label='noisy data');ax.scatter(x, V_GPS[interval], s=0.1, label='original')
x_line = x;y_line = np.array([np.ones_like(x),x]).transpose().dot(β);ax.plot(x_line, y_line, linewidth=1, color="r",label='linear fit');ax.set_xlabel("$V_{odom}$");ax.set_ylabel("$V_{GPS}$");ax.legend(loc='best');ax.set_title("Linear fit with RMS = %.2f" % rms);

Results for different cases

\begin{array}{|l|c|c|c|} \hline & C_0 & C_1 & RMS \\ \hline GPS & 2.01 & 0.8 & 0.085 \\ \hline GPS, noisy & 2.75 & 0.75 & 0.29 \\ \hline GPS, noisy, cutted~ends & 2.09 & 0.8 & 0.2 \\ \hline \end{array}

Thank you for listening!